//
//  engine.cpp
//  template4
//
//  Created by Xiaochen Lian on 5/15/12.
//  Copyright (c) 2012 University of California, Los Angeles. All rights reserved.
//

#include "engine.h"

CEngine::CEngine( /*GLfloat fovy, GLfloat aspect, GLfloat zNear, GLfloat zFar*/)
{
    //        this->fovy = fovy;
    //        this->aspect = aspect;
    //        this->zNear = zNear;
    //        this->zFar = zFar;
    body = NULL;
    INIT_PHI = M_PI/2;
    phi = INIT_PHI;
    phi_step = 10.0;
    current_params.num_params = 11;
}

void CEngine::init()
{
    program_axis = InitShader( "vshader_axis.glsl", "fshader_axis.glsl" );
    glUseProgram(program_axis);
    uModelView_axis  = glGetUniformLocation( program, "ModelView"  );
    uProjection_axis = glGetUniformLocation( program, "Projection" );
    uCameraView_axis = glGetUniformLocation( program, "CameraView" );
    
    program = InitShader( "vshader.glsl", "fshader.glsl" );
    glUseProgram(program);
    
    uModelView  = glGetUniformLocation( program, "ModelView"  );
    uProjection = glGetUniformLocation( program, "Projection" );
    uCameraView = glGetUniformLocation( program, "CameraView" );
    
    glClearColor( 0.1, 0.1, 0.2, 1.0 ); // dark blue background
    
    uAmbient   = glGetUniformLocation( program, "AmbientProduct"  );
    uDiffuse   = glGetUniformLocation( program, "DiffuseProduct"  );
    uSpecular  = glGetUniformLocation( program, "SpecularProduct" );
    uLightPos  = glGetUniformLocation( program, "LightPosition"   );
    uShininess = glGetUniformLocation( program, "Shininess"       );
    uTex       = glGetUniformLocation( program, "Tex"             );
    uEnableTex = glGetUniformLocation( program, "EnableTex"       );
    
    glEnable(GL_DEPTH_TEST);
}

void CEngine::build_scene()
{
    /*-------------------------------------------------*/
    /*                     Environment                 */
    /*-------------------------------------------------*/
    eye = vec4(200.0, 0.0, 0.0, 0.0);
    ref = vec4(0.0);
    up = vec4(0.0, 1.0, 0.0, 0.0);
    mat4 view = LookAt(eye, ref, up);
    glUniformMatrix4fv( uCameraView, 1, GL_TRUE, view );
    
    glUniform4f(uLightPos,  15.0f, 15.0f, 30.0f, 1.0f);
    g_env_lighting.ambient = color4(0.2f,  0.2f,  0.2f, 1.0f);
    g_env_lighting.diffuse = color4(0.6f,  0.6f,  0.6f, 1.0f);
    g_env_lighting.specular = color4(0.2f,  0.2f,  0.2f, 1.0f);
    g_env_lighting.shininess = 100.0f;
    
    GRAVITY = 9.8;
    
    aero_coef.rho = 1.0;
    aero_coef.x1 = -0.5;
    aero_coef.x3 = 0.7;
    
    /*-------------------------------------------------*/
    /*                     Modeling                    */
    /*-------------------------------------------------*/
    // Generate vertex arrays for geometric shapes
    vec3 body_scale(3.0, 3.0, 10.0);
    generateCube(program, &cubeData);
    generateSphere(program, &sphereData);
    generateEllipsoid(program, &ellipsData, body_scale);
    generateCone(program, &coneData);
    generateCylinder(program, &cylData);
    
    
    // parameters for shapes
    const GLfloat arm_length = 5.0f;
    const GLfloat arm_loc = 0.5;
//    const GLfloat forearm_length = 5.0f;
    const GLfloat wrist_length = 7.0f;
    const GLfloat feather_length = body_scale[2]*2/3;
    const GLfloat tail_length = body_scale[2] / 2;
    
    // parameters for positions
    current_status.shoulder_sweep[1] = 0;          current_status.shoulder_sweep[2] = 0;
    
    current_status.lshoulder_dihedral[1] = -30;       current_status.lshoulder_dihedral[2] = 30;
    current_status.lshoulder_twist[1] = 0;          current_status.lshoulder_twist[2] = 0;
    current_status.lwrist_bend[1] = 0;              current_status.lwrist_bend[2] = 0;
    current_status.lwrist_twist[1] = 0;              current_status.lwrist_twist[2] = 0;
    
    current_status.rshoulder_dihedral[1] = 30;       current_status.rshoulder_dihedral[2] = -30;
    current_status.rshoulder_twist[1] = 0;          current_status.rshoulder_twist[2] = 0;
    current_status.rwrist_bend[1] = 0;              current_status.rwrist_bend[2] = 0;
    current_status.rwrist_twist[1] = 0;              current_status.rwrist_twist[2] = 0;    
    
    current_status.tail_bend[1] = 0;                current_status.tail_bend[2] = 0;
    current_status.tail_twist[1] = 0;               current_status.tail_twist[2] = 0;

    
    // body    
    body = new CBody(body_scale, vec4(0.0f));
    body->build_model(vec4(0.0, 0.0, 0.0, 1.0));
    // arm
    GLfloat x = sqrt(1 - arm_loc*arm_loc) * body_scale[0];
    GLfloat y = 0;
    GLfloat z = body_scale[2] * arm_loc;

    vec4 pos = vec4(x, 0.0f, z, 0.0f);
    CArm *left_arm = new CArm(pos, arm_length);
    body->add_child(left_arm);
    
    pos = vec4(-x, 0.0f, z, 0.0f); 
    CArm *right_arm = new CArm(pos, -arm_length); // negative length indicates right side
    body->add_child(right_arm);
    
    // wrist
    CWrist *left_wrist = new CWrist(vec3(arm_length/2, 0.0f, 0.0f), wrist_length);
    left_arm->add_child(left_wrist);
    
    CWrist *right_wrist = new CWrist(vec3(-arm_length/2, 0.0f, 0.0f), -wrist_length);
    right_arm->add_child(right_wrist);
    
    // feathers
    CFeather *larm_feather = new CFeather(vec3(0.0f, y, -feather_length/2), arm_length, feather_length);
    left_arm->add_child(larm_feather);
    
    CFeather *lwrist_feather = new CFeather(vec3(0.0f, y, -feather_length/2), wrist_length, feather_length);
    left_wrist->add_child(lwrist_feather);
    
    CFeather *rarm_feather = new CFeather(vec3(0.0f, y, -feather_length/2), arm_length, feather_length);
    right_arm->add_child(rarm_feather);
    
    CFeather *rwrist_feather = new CFeather(vec3(0.0f, y, -feather_length/2), wrist_length, feather_length);
    right_wrist->add_child(rwrist_feather);
    
    CFeather *tail_feather = new CFeather(vec3(0.0f, y, -body_scale[2]), body_scale[0]*2, tail_length);
    body->add_child(tail_feather);
    
    // mass
    bird.mass = 100;
}

void CEngine::init_bird()
{    
    phi = INIT_PHI;
    
    current_status.shoulder_sweep[0] = 0;
    
    current_status.lshoulder_dihedral[0] = 0;
    current_status.lshoulder_twist[0] = 0;
    current_status.lwrist_bend[0] = 0;
    current_status.lwrist_twist[0] = 0;
    
    current_status.rshoulder_dihedral[0] = 0;
    current_status.rshoulder_twist[0] = 0;
    current_status.rwrist_bend[0] = 0;
    current_status.rwrist_twist[0] = 0;
    
    current_status.tail_bend[0] = 0;
    current_status.tail_twist[0] = 0;
    
    current_status.pos = 0.0f;
    current_status.velocity = 0.0f;
    current_status.accelerate = 0.0f;
}

void CEngine::display()
{
    glUseProgram(program);
    body->draw(model_view);
}

void CEngine::display_axis()
{
    glUseProgram(program_axis);
    body->draw_axis(model_view);
}

void CEngine::update(GLdouble timestep)
{
    timestep = 0.001;
    phi += timestep * phi_step;
    if (phi > 2*M_PI || phi < 0)
        phi_step = -phi_step;
    
    for (int i = 0; i < current_params.num_params; ++i)
        current_params.u[i] = (1+cos(phi)) * 0.5;

    bird_status_by_params(current_status, current_params);
    aerodynamics(current_status, current_params, timestep);
    update_bird_status(timestep);
    
    body->update(current_status, model_view);
}

void CEngine::bird_dof_by_params(const bird_parameters &cparams)
{   
    current_dof.shoulder_sweep = cparams.u[0];
    
    current_dof.lshoulder_dihedral = cparams.u[1];
    current_dof.lshoulder_twist = cparams.u[2];
    current_dof.lwrist_bend = cparams.u[3];
    current_dof.lwrist_twist = cparams.u[4];
    
    current_dof.rshoulder_dihedral = cparams.u[5];
    current_dof.rshoulder_twist = cparams.u[6];
    current_dof.rwrist_bend = cparams.u[7];
    current_dof.rwrist_twist = cparams.u[8];
    
    current_dof.tail_bend = cparams.u[9];
    current_dof.tail_twist = cparams.u[10];
}

void CEngine::bird_status_by_params(bird_status &cstatus, const bird_parameters &cparams)
{
    cstatus.status_by_dof(cstatus.shoulder_sweep, cparams.u[0]);

    cstatus.status_by_dof(cstatus.lshoulder_dihedral, cparams.u[1]);
    cstatus.status_by_dof(cstatus.lshoulder_twist, cparams.u[2]);
    cstatus.status_by_dof(cstatus.lwrist_bend, cparams.u[3]);
    cstatus.status_by_dof(cstatus.lwrist_twist, cparams.u[4]);
    
    cstatus.status_by_dof(cstatus.rshoulder_dihedral, cparams.u[5]);
    cstatus.status_by_dof(cstatus.rshoulder_twist, cparams.u[6]);
    cstatus.status_by_dof(cstatus.rwrist_bend, cparams.u[7]);
    cstatus.status_by_dof(cstatus.rwrist_twist, cparams.u[8]);
    
    cstatus.status_by_dof(cstatus.tail_bend, cparams.u[9]);
    cstatus.status_by_dof(cstatus.tail_twist, cparams.u[10]);
}

void CEngine::aerodynamics(const bird_status &status, const bird_parameters &cparams, const double timestep)
{
//    bird_acc acc;
//    acc.pacX = 0;
//    acc.pacY = 0;
//    acc.pacZ = 0;
//    acc.aacX = 0;
//    acc.aacY = 0;
//    acc.aacZ = 0;
//    return acc;
    vec4 force;
    
    force = body->aerodynamics(status, mat4(1.0), timestep);
    force += vec4(0.0, -GRAVITY * bird.mass, 0.0, 0.0);
    current_status.accelerate = force / bird.mass;
}

void CEngine::update_bird_status(GLdouble timestep)
{
    current_status.pos += current_status.velocity * timestep;
//    current_status.posX += current_status.spdX * timestep;
//    current_status.posY += current_status.spdY * timestep;
//    current_status.posZ += current_status.spdZ * timestep;
    
//    current_status.rotX += current_status.aspX * timestep;
//    current_status.rotY += current_status.aspY * timestep;
//    current_status.rotZ += current_status.aspZ * timestep;

    current_status.velocity += current_status.accelerate * timestep;
//    current_status.spdX += current_status.pacX * timestep;
//    current_status.spdY += current_status.pacY * timestep;
//    current_status.spdZ += current_status.pacZ * timestep;
    
//    current_status.aspX += current_status.aacX * timestep;
//    current_status.aspY += current_status.aacY * timestep;
//    current_status.aspZ += current_status.aacZ * timestep;
}

//    void rotate_camera(const GLfloat dpitch, const GLfloat dyaw)
//    {
//        camera.pitch += dpitch;
//        camera.yaw += dyaw;
//    }
//    
//    void move_camera(int d)
//    {
//        GLfloat unit = 4.0;
//        switch (d) {
//            case 0:// w
//                camera.eye.z -= cos(camera.pitch*DegreesToRadians)*cos(-camera.yaw*DegreesToRadians) * unit;
//                camera.eye.x += cos(camera.pitch*DegreesToRadians)*sin(-camera.yaw*DegreesToRadians) * unit;
//                camera.eye.y += sin(camera.pitch*DegreesToRadians) * unit;
//                break;
//            case 1://a
//                camera.eye.x -= cos(camera.pitch*DegreesToRadians)*cos(camera.yaw*DegreesToRadians) * unit;
//                camera.eye.z += cos(camera.pitch*DegreesToRadians)*sin(camera.yaw*DegreesToRadians) * unit;
//                break;
//            case 2://d
//                camera.eye.x += cos(camera.pitch*DegreesToRadians)*cos(camera.yaw*DegreesToRadians) * unit;
//                camera.eye.z -= cos(camera.pitch*DegreesToRadians)*sin(camera.yaw*DegreesToRadians) * unit;
//                break;
//            case 3://s
//                camera.eye.z += cos(camera.pitch*DegreesToRadians)*cos(-camera.yaw*DegreesToRadians) * unit;
//                camera.eye.x -= cos(camera.pitch*DegreesToRadians)*sin(-camera.yaw*DegreesToRadians) * unit;
//                camera.eye.y -= sin(camera.pitch*DegreesToRadians) * unit;
//                break;
//            default:
//                break;
//        }
//    }
//
//    void set_aspect(GLfloat aspect)
//    {
//        this->aspect = aspect;
//    }
